//
// Created by LiuDongPeng on 2024/1/29.
//

#include "motor.h"
#include "foc.h"



Motor::Motor()
{
    0;
}

bool Motor::link_encoder(EncoderBase *encoder)
{
    return (encoder_ = encoder) != nullptr;
}

bool Motor::has_encoder() const
{
    return encoder_ != nullptr;
}

EncoderBase *Motor::get_encoder() const
{
    return encoder_;
}

void Motor::open_loop_test(float ud, float uq)
{
    if (!has_encoder())
        return;

    /* 1. clark */
    foc_clark(ia_, ib_, &ialpha_, &ibeta_);

    /* 2. park */
    foc_park(ialpha_, ibeta_, sinTheta_, cosTheta_, &id_, &iq_);

    /* 4. inv park */
//    uq *= 1.0f / (2.0f / 3.0f * VBus_);
//    ud *= 1.0f / (2.0f / 3.0f * VBus_);
    foc_inv_park(ud, uq, sinTheta_, cosTheta_, &ualpha_, &ubeta_);

    /* 5. svpwm */
//    auto [valid, ta, tb, tc] = odriver_svm(ualpha_, ubeta_);
    auto [valid, ta, tb, tc] = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = ta;
    tb_ = tb;
    tc_ = tc;

    if (valid)
    {
        apply_pwm_duty(ta, tb, tc);
    }
}

void Motor::idq_pi_ctrl(float idRef, float iqRef)
{

}

void Motor::current_closed_loop(float idRef, float iqRef)
{
    if (!has_encoder())
        return;

    /* 1. clark */
    foc_clark(ia_, ib_, &ialpha_, &ibeta_);

    /* 2. park */
    foc_park(ialpha_, ibeta_, sinTheta_, cosTheta_, &id_, &iq_);

    /* 3. id iq pid ctrl */
    idq_pi_ctrl(idRef, iqRef);

    /* 4. inv park */
    uq_ *= 1.0f / (2.0f / 3.0f * VBus_);
    ud_ *= 1.0f / (2.0f / 3.0f * VBus_);
    foc_inv_park(ud_, uq_, sinTheta_, cosTheta_, &ualpha_, &ubeta_);

    /* 5. svpwm */
    auto [valid, ta, tb, tc] = odriver_svm(ualpha_, ubeta_);
    if (valid)
    {
        apply_pwm_duty(ta, tb, tc);
    }
}

void Motor::speed_closed_loop(float speedRef)
{

}

void Motor::position_closed_loop(float posRef)
{

}

void Motor::align_encoder(float ud, float angle)
{
    this->enable();

    /* 给ud，电角度设置为0，转子会转到电气零位 */
//    foc_inv_park(ud, 0, arm_sin_f32(angle), arm_cos_f32(angle),
//				 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);
//    motor_svpwm1(motor);

    float offset1 = 0, offset2 = 0, offset3 = 0;
    std::tuple<bool, float, float, float> svmVal;
    std::tuple<bool, uint32_t, float> encoderVal;

    // 第一次定位到0°
    foc_inv_park(ud, 0, arm_sin_f32(0), arm_cos_f32(0), &ualpha_, &ubeta_);
    svmVal = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = std::get<1>(svmVal);
    tb_ = std::get<2>(svmVal);
    tc_ = std::get<3>(svmVal);
    if (std::get<0>(svmVal))
    {
        apply_pwm_duty(ta_, tb_, tc_);
    }
    HAL_Delay(500);
    encoderVal = encoder_->update();
    offset1 = std::get<2>(svmVal);

    // 定位到+90°
    foc_inv_park(ud, 0, arm_sin_f32(0.5f * M_PI), arm_cos_f32(0.5f * M_PI), &ualpha_, &ubeta_);
    svmVal = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = std::get<1>(svmVal);
    tb_ = std::get<2>(svmVal);
    tc_ = std::get<3>(svmVal);
    if (std::get<0>(svmVal))
    {
        apply_pwm_duty(ta_, tb_, tc_);
    }
    HAL_Delay(500);

    // 第二次定位到0°
    foc_inv_park(ud, 0, arm_sin_f32(0), arm_cos_f32(0), &ualpha_, &ubeta_);
    svmVal = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = std::get<1>(svmVal);
    tb_ = std::get<2>(svmVal);
    tc_ = std::get<3>(svmVal);
    if (std::get<0>(svmVal))
    {
        apply_pwm_duty(ta_, tb_, tc_);
    }
    HAL_Delay(500);
    encoderVal = encoder_->update();
    offset2 = std::get<2>(svmVal);

    // 定位到-90°
    foc_inv_park(ud, 0, arm_sin_f32(-0.5f * M_PI), arm_cos_f32(-0.5f * M_PI), &ualpha_, &ubeta_);
    svmVal = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = std::get<1>(svmVal);
    tb_ = std::get<2>(svmVal);
    tc_ = std::get<3>(svmVal);
    if (std::get<0>(svmVal))
    {
        apply_pwm_duty(ta_, tb_, tc_);
    }
    HAL_Delay(500);

    // 第三次定位到0°
    foc_inv_park(ud, 0, arm_sin_f32(0), arm_cos_f32(0), &ualpha_, &ubeta_);
    svmVal = mid_point_svm(ualpha_, ubeta_, VBus_);
    ta_ = std::get<1>(svmVal);
    tb_ = std::get<2>(svmVal);
    tc_ = std::get<3>(svmVal);
    if (std::get<0>(svmVal))
    {
        apply_pwm_duty(ta_, tb_, tc_);
    }
    HAL_Delay(500);
    encoderVal = encoder_->update();
    offset3 = std::get<2>(svmVal);

    angleRadOffset_ = (offset1 + offset2 + offset3) / 3.0f;

    this->disable();
}

void Motor::enable()
{
    status_ = MotorStatus::MOTOR_STATUS_RUN;

    HAL_TIM_PWM_Start(tim_, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(tim_, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(tim_, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(tim_, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(tim_, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(tim_, TIM_CHANNEL_3);
}

void Motor::disable()
{
    status_ = MotorStatus::MOTOR_STATUS_STOP;

    HAL_TIM_PWM_Stop(tim_, TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(tim_, TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(tim_, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Stop(tim_, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Stop(tim_, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Stop(tim_, TIM_CHANNEL_3);
}

void Motor::apply_pwm_duty(float ta, float tb, float tc) const
{
    __HAL_TIM_SET_COMPARE(tim_, TIM_CHANNEL_1, (uint32_t)(ta * timArr_));
    __HAL_TIM_SET_COMPARE(tim_, TIM_CHANNEL_2, (uint32_t)(tb * timArr_));
    __HAL_TIM_SET_COMPARE(tim_, TIM_CHANNEL_3, (uint32_t)(tc * timArr_));
}

void Motor::set_ia_ib_ic(float ia, float ib, float ic)
{
    ia_ = ia;
    ib_ = ib;
    ic_ = ic;
}

Motor::MotorStatus Motor::get_status() const
{
    return status_;
}

void Motor::set_status(Motor::MotorStatus status)
{
    status_ = status;
}

std::tuple<float, float> Motor::calc_sin_cos_theta()
{
    sinTheta_ = arm_sin_f32(theta_);
    cosTheta_ = arm_cos_f32(theta_);
    return std::make_tuple(sinTheta_, cosTheta_);
}

std::tuple<float, float> Motor::sin_cos_theta(float theta)
{
    return std::make_tuple(arm_sin_f32(theta), arm_cos_f32(theta));
}

float Motor::getAngleRad() const
{
    return angleRad_;
}

void Motor::setAngleRad(float angleRad)
{
    angleRad_ = angleRad;
}

float Motor::getAngleRadOffset() const
{
    return angleRadOffset_;
}

void Motor::setAngleRadOffset(float angleRadOffset)
{
    angleRadOffset_ = angleRadOffset;
}

float Motor::getTheta()
{
    float val;
    if (angleRad_ >= angleRadOffset_)
    {
        val = angleRad_ - angleRadOffset_;
    }
    else
    {
        val = (float) M_TWOPI - angleRadOffset_ + angleRad_;
    }
    theta_ = fmodf(val * (float) polePairs_, (float) M_TWOPI);

    return theta_;
}

void Motor::setTheta(float theta)
{
    theta_ = theta;
}

TIM_HandleTypeDef *Motor::getTim() const
{
    return tim_;
}

void Motor::setTim(TIM_HandleTypeDef *tim)
{
    tim_ = tim;
}
